#include "FLAlgorithm.hpp"

const double FLAlgorithm::offset = 3.14/4; // turn if more than 45 deg from center

FLAlgorithm::FLAlgorithm(): SwarmAlgorithm() {
    id = "FOLLOWLEADER";
}

bool FLAlgorithm::setup() {
    turnLEDsOff();

    color = readMessageStr();
    led = NULL;
    while (led == NULL) {
        led = getLED(color);
    }
	
    string inst = readMessageStr();
    bool isLeader = SwarmOS::getNetworkID() == atoi(inst.c_str());

    if(isLeader)
    {
        led->setLED(true);
        isRunning = false;
    }

    return true;
}

void FLAlgorithm::loopBody() {
    static int c = LED::colorStringToInt(color);
    static int intelCount = 0; // how long to continue last intelligence

    // see the world Barnaby
    robots = robotFinder->getRobotPositions();
    unsigned char action = ACTION_STOP;

    led->setLED(false); // reset LED; only goes back on if leader found ahead

    // handle bumper issues
    if(bumperControl->isFrontTouching())
    {
        if (bumperControl->isBackTouching())
        {
            motorControl->stop();
            return;
        }

        int i = getBumperRobotIndex(robots);
        if (i > -1 && strcmp(color.c_str(), robots[i].label) == 0) { // bumping our robot
            motorControl->stop();
            led->setLED(true);
            return;
        }
        
        // bumping another robot
        action = ACTION_REV_LT;
    }
    else if (bumperControl->isBackTouching()) {
        int i = getBumperRobotIndex(robots, false);
        if (i > -1 && strcmp(color.c_str(), robots[i].label) == 0) {
            motorControl->stop();
            led->setLED(true);
            return;
        }

        action = ACTION_TURN_RT;
    }
    else {
        unsigned char newAction = ClusterAlgorithm::clusterClosestAction(c);
        if(newAction != ACTION_STOP)
        {
            action = newAction;
            led->setLED(action == ACTION_FORWARD);
            intelCount = 3;
        }
        else if (intelCount > 0) {
            intelCount--;
            led->setLED(action == ACTION_FORWARD);
        }
        else {
            action = getRandomMotorAction();
        }
    }

    performAction(action, 5, 5);
}
